#include "TXLib.h"

const int MATRIX_SIZE = 4;

void mat_mat (const double input_a [MATRIX_SIZE] [MATRIX_SIZE],
              const double input_b [MATRIX_SIZE] [MATRIX_SIZE],
              double output [MATRIX_SIZE] [MATRIX_SIZE]);

void mat_vec (const double input_mat [MATRIX_SIZE] [MATRIX_SIZE],
              const double input_vec [MATRIX_SIZE],
              double output_vec [MATRIX_SIZE]);

void mat_cpy (const double input_matrix [4] [4], double output_matrix [4] [4]);



void mat_mat (const double input_a [MATRIX_SIZE] [MATRIX_SIZE],
              const double input_b [MATRIX_SIZE] [MATRIX_SIZE],
              double output [MATRIX_SIZE] [MATRIX_SIZE])
{
    for (int output_row = 0; output_row < MATRIX_SIZE; output_row ++)
    {
        for (int output_column = 0; output_column < MATRIX_SIZE; output_column ++)
        {
            output [output_row] [output_column] = 0;

            int a_column = 0;
            int b_row = 0;

            for (int i = 0; i < MATRIX_SIZE; i ++)
            {
                a_column = i;
                b_row = i;

                output [output_row] [output_column] += input_a [output_row] [a_column] * input_b [b_row] [output_column];
            }
        }
    }
}

void mat_vec (const double input_mat [MATRIX_SIZE] [MATRIX_SIZE],
              const double input_vec [MATRIX_SIZE],
              double output_vec [MATRIX_SIZE])
{
    for (int row = 0; row < MATRIX_SIZE; row ++)
    {
        for (int column = 0; column < MATRIX_SIZE; column ++)
        {
            output_vec [row] += input_mat [row] [column] * input_vec [column];
        }
    }
}

void mat_cpy (const double input_matrix [4] [4], double output_matrix [4] [4])
{
    for (int row = 0; row < 4; row ++)
    {
        for (int column = 0; column < 4; column ++)
        {
            output_matrix [row] [column] = input_matrix [row] [column];
        }
    }
}
